#!/usr/bin/python
import roslib; roslib.load_manifest('DelphinROSv2')
import rospy
import csv
import time
import numpy
import os
from datetime import datetime

from DelphinROSv2.msg import compass
from DelphinROSv2.msg import tsl_setpoints
from DelphinROSv2.msg import tsl_feedback
from std_msgs.msg import Float32
from std_msgs.msg import Int8
from std_msgs.msg import String
from DelphinROSv2.msg import tail_setpoints
from DelphinROSv2.msg import tail_feedback
from DelphinROSv2.msg import position
from DelphinROSv2.msg import gps
from DelphinROSv2.msg import altitude
from DelphinROSv2.msg import sonar
from DelphinROSv2.msg import depth_control


global compassWriter
global compassFile
global tslHorzWriter
global tslHorzFile
global tslHorz
global tslVertWriter
global tslVertFile
global tslVert
global tsl_feedback
global heading_demand
global depth_demand
global prop_demand
global tailFile
global tailWriter
global tail_sp
global positionFile
global positionWriter
global gpsFile
global altimeterFile
global sonarFile
global sonarWriter
global time_zero

global log_folder

def callback_compass(compass_data):
    #outputs compass data to file: time, heading, roll, pitch, temperature, depth, m, mx, my, mz, a, ax, ay, az 
    stringtime = time.time()-time_zero
    compassList = [stringtime, compass_data.heading, compass_data.roll, compass_data.pitch, compass_data.temperature, compass_data.depth, compass_data.m, compass_data.mx, compass_data.my, compass_data.mz, compass_data.a, compass_data.ax, compass_data.ay, compass_data.az]
    #print 'logging compass!'
    try:
        compassWriter.writerow(compassList)
    except ValueError:
        print 'writerow error'
        
##############################################################
    
def horiz_callback(tsl_horiz_data):
    #output horizontal thruster data to file:  time, heading_demand, thurster2 setpoint, thruster3 setpoint, pterm, iterm, dterm
    global tsl_Horz
    tsl_Horz = tsl_horiz_data        
        
def vert_callback(tsl_vert_data):
    #output vertical thruster data to file:  time, heading_demand, thurster0 setpoint, thruster1 setpoint, pterm, iterm, dterm
    global tsl_Vert
    tsl_Vert = tsl_vert_data
        
def tsl_feedback_callback(data):
    #updates heading_demand - to be included in horizontal thruster log file
    #global tsl_feedback
    #tsl_feedback = data
    
    stringtime = time.time()-time_zero
    tslVertList = [stringtime, depth_demand, tsl_Vert.thruster0, data.speed0, data.current0, tsl_Vert.thruster1, data.speed1, data.current1, data.voltage]
    try:
        tslVertWriter.writerow(tslVertList)
    except ValueError:
        print 'writerow error'
        
    tslHorzList = [stringtime, heading_demand, tsl_Horz.thruster0, data.speed2, data.current2, tsl_Horz.thruster1, data.speed3, data.current3, data.voltage]
    try:
        tslHorzWriter.writerow(tslHorzList)
    except ValueError:
        print 'writerow error'
    
def headingdemand_callback(data):
    #updates heading_demand - to be included in horizontal thruster log file
    global heading_demand
    heading_demand = data.data
        
def depthdemand_callback(data):
    #updates depth_demand - to be included in vertical thruster log file
    global depth_demand
    depth_demand = data.data
    
def tail_feedback_callback(data):
    #output tail section data to file: time, b_sp, b_feedback, c_sp, c_feedback, d_sp, d_feedback, e_sp, e_feedback, prop_sp, prop_feedback
    stringtime = time.time()-time_zero
    tailList = [stringtime, data.bsp, data.b, data.csp, data.c, data.dsp, data.d, data.esp, data.e, prop_demand, data.current, data.rpm]
    try:
        tailWriter.writerow(tailList)
    except ValueError:
        print 'writerow error'

def propdemand_callback(data):
    #updates prop_demand - to be included in tail section log file
    global prop_demand
    prop_demand = data.data

def position_callback(position):
    stringtime = time.time()-time_zero
    positionList = [stringtime, position.X, position.Y, position.Z, position.forward_vel, position.sway_vel]
    try:
        positionWriter.writerow(positionList)
    except ValueError:
        print 'writerow error'


def gps_callback(gps):
    stringtime = time.time()-time_zero
    gpsList = [stringtime, gps.latitude, gps.longitude, gps.time, gps.number_of_satelites, gps.fix,gps.speed,gps.x,gps.y]
    try:
        gpsWriter.writerow(gpsList)
    except ValueError:
        print 'writerow error'

def altimeter_callback(altimeter):
    stringtime = time.time()-time_zero
    altimeterList=[stringtime, altimeter.altitude]
    try:
        altimeterWriter.writerow(altimeterList)
    except ValueError:
        print 'writerow error'

def sonar_callback(sonar): ## UNTESTED!!!!
    #print 'SONAR CALLBAK'
    stringtime = time.time()-time_zero
    dataList = list(numpy.fromstring(sonar.data, dtype = numpy.uint8))
    print dataList
    sonarData = [stringtime] + dataList
    print sonarData
    try:
      #  print 'write'
        sonarWriter.writerow(sonarData)
    except ValueError:
        print 'writerow error in sonar'
        
def depthcontrol_callback(DC): 
    stringtime = time.time()-time_zero
    depthcontrolList = [stringtime, DC.depth, DC.pitch, DC.speed, DC.depth_demand, DC.error, DC.int_error, DC.der_error, DC.min_int_error, DC.max_int_error, DC.CL, DC.buoyancy, DC.CS_pitch_Pgain, DC.CS_pitch_Igain, DC.CS_pitch_Pmin, DC.CS_pitch_Pmax, DC.CS_pitch_Imin, DC.CS_pitch_Imax, DC.CS_pitch_min, DC.CS_pitch_max, DC.CS_Pgain, DC.CS_Igain, DC.CS_min, DC.CS_max, DC.CS_ss_pitch_angle, DC.CS_pitch_Pterm, DC.CS_pitch_Iterm, DC.CS_pitch_demand, DC.pitch_error, DC.CS_Pterm, DC.CSp, DC.CSs, DC.estLiftForce, DC.Thrust_Pgain, DC.Thrust_Igain, DC.Thrust_Dgain, DC.Thrust_Pmin, DC.Thrust_Pmax, DC.Thrust_Imin, DC.Thrust_Imax, DC.Thrust_Dmin, DC.Thrust_Dmax, DC.Thrust_Smin, DC.Thrust_Smax, DC.Thrust_Pterm, DC.Thrust_Iterm, DC.Thrust_Dterm, DC.Thrust_total, DC.thruster0, DC.thruster1]
    
    try:
        depthcontrollerWriter.writerow(depthcontrolList)
    except ValueError:
        print 'writerow error'

def shutdown():
    #shutdown behaviour - close all files
    #print 'shutting down'
    compassFile.close()
    tslHorzFile.close()
    tslVertFile.close()
    tailFile.close()
    positionFile.close()
    gpsFile.close()
    altimeterFile.close()
    sonarFile.close()
    depthcontrollerFile.close()


if __name__ == '__main__':
    rospy.init_node('Logger')
    
    stringtime = datetime.now()
    stringtime = stringtime.strftime('%Y-%m-%d_%H-%M-%S')
    rospy.loginfo('Logger started at %s.'%(stringtime))
    
    global heading_demand
    heading_demand = -1
    global depth_demand
    depth_demand = -1
    global prop_demand
    prop_demand = -1
    global tail_horizontal_setpoints
    tail_horizontal_setpoints = tail_setpoints()
    global tail_vertical_setpoints
    tail_vertical_setpoints = tail_setpoints()
    global tsl_Vert
    tsl_Vert = tsl_setpoints()
    global tsl_Horz
    tsl_Horz = tsl_setpoints()
    global time_zero
    time_zero = time.time()
    
    #define files and writers
    dirname = stringtime
    if not os.path.isdir(dirname):
        os.mkdir(dirname)
        
    compassFile  = open('%s/compassLog.csv' %(dirname), "w")
    compassWriter = csv.writer(compassFile, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
    tslHorzFile  = open('%s/tslHorzLog.csv' %(stringtime), "w")
    tslHorzWriter = csv.writer(tslHorzFile, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
    tslVertFile  = open('%s/tslVertLog.csv' %(stringtime), "w")
    tslVertWriter = csv.writer(tslVertFile, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
    tailFile  = open('%s/tailLog.csv' %(stringtime), "w")
    tailWriter = csv.writer(tailFile, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
    positionFile  = open('%s/positionLog.csv' %(stringtime), "w")
    positionWriter = csv.writer(positionFile, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
    gpsFile  = open('%s/gpsLog.csv' %(stringtime), "w")
    gpsWriter = csv.writer(gpsFile, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
    altimeterFile=open('%s/altimeterLog.csv' %(stringtime), "w")
    altimeterWriter=csv.writer(altimeterFile, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
    sonarFile = open('%s/sonarLog.csv' %(stringtime), "w")
    sonarWriter=csv.writer(sonarFile, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
    depthcontrollerFile = open('%s/depthcontrolLog.csv' %(stringtime), "w")
    depthcontrollerWriter=csv.writer(depthcontrollerFile, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)

    #set up subscribers...
    rospy.Subscriber('compass_out', compass, callback_compass)
    rospy.Subscriber('TSL_setpoints_horizontal', tsl_setpoints, horiz_callback)
    rospy.Subscriber('TSL_setpoints_vertical', tsl_setpoints, vert_callback)    
    rospy.Subscriber('TSL_feedback', tsl_feedback, tsl_feedback_callback)        
    rospy.Subscriber('heading_demand', Float32, headingdemand_callback)
    rospy.Subscriber('depth_demand', Float32, depthdemand_callback)
    rospy.Subscriber('prop_demand', Int8, propdemand_callback)
    rospy.Subscriber('tail_output', tail_feedback, tail_feedback_callback)
    rospy.Subscriber('position_dead', position, position_callback)
    rospy.Subscriber('gps_out', gps, gps_callback)
    rospy.Subscriber('altimeter_out',altitude, altimeter_callback)
    rospy.Subscriber('sonar_output', String, sonar_callback)
    rospy.Subscriber('Depth_controller_values', depth_control, depthcontrol_callback)
    
    #keep node running until shut down
    str = "Logger online - output directory: %s" %(dirname)
    rospy.loginfo(str)
    
    rospy.on_shutdown(shutdown)

    rospy.spin()
    
